#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64MultiArray
import argparse

def main():
    parser = argparse.ArgumentParser(description='Set joint positions.')
    parser.add_argument('joint_positions', type=float, nargs='*',
                        help='7 joint positions to command (if none provided, moves to home position)')
    args = parser.parse_args()

    rospy.init_node('move_joint_commander', anonymous=True)
    pub = rospy.Publisher('/command_move_joint', Float64MultiArray, queue_size=10)

    rospy.sleep(1)
    msg = Float64MultiArray()
    
    # If exactly 7 arguments provided, use them
    if len(args.joint_positions) == 7:
        msg.data = args.joint_positions
        rospy.loginfo(f"Moving to joint positions: {msg.data}")
    # If no arguments provided, move to all zeros
    else:
        msg.data = [0.0] * 7
        rospy.loginfo("No arguments provided. Moving to home position [0,0,0,0,0,0,0]")

    pub.publish(msg)

if __name__ == '__main__':
    main()